// Copyright (c) 2012, Hazen Eckert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the The University of Texas at Dallas nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT DALLAS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <ros/ros.h>
#include <auv_motor_driver/motor_driver.h>
#include <auv_motor_driver/motor_driver_node.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <dynamic_reconfigure/server.h>
#include <auv_motor_driver/auvConfig.h>
#include <auv_msgs/MotorPWM.h>
#include <auv_msgs/MotorCurrent.h>


motor_driver_node::motor_driver_node(ros::NodeHandle h):auv_driver_node(h), server()
{
    state = auv_driver_node::CLOSED;
    
    driver = new motor_driver;
    
  	dynamic_reconfigure::Server<auv_motor_driver::auvConfig>::CallbackType f;
  	f = boost::bind(&motor_driver_node::reconfigureCallback, this, _1, _2);
    server.setCallback(f);
    
    private_nh.param<std::string>("port", port, "/dev/motor_controller");
    try {
    	driver->open(port.c_str(), 115200);
    	state = auv_driver_node::OPENED;
    	getDriverInfo();
    } catch (int e) {
    	if (e == 19){
    		ROS_ERROR("Open FAILED");
    	}
    	state = auv_driver_node::CLOSED;
    }
    
    sendErrorCodesToDiagnostics(0xff << 8);
    
    updater.add("Motor Diagnostics", this, &motor_driver_node::update_motor_diagnostics);
    
    for (int i = 0; i < 6; i++) {
    		motorPWMs[i] = 0;
    }
    
    motorPWM_sub = nh.subscribe( "motorPWM", 10, &motor_driver_node::setMotorPWM, this);
    motorCurrent_pub = nh.advertise<auv_msgs::MotorCurrent>("motorCurrent", 10);
    
}

motor_driver_node::~motor_driver_node()
{
    delete driver;
}

void motor_driver_node::setMotorPWM(const auv_msgs::MotorPWM::ConstPtr& msg)
{

    for (int i = 0; i < 6; i++) {
    		motorPWMs[i] = msg->motorPWMs[i];
        if (motorPWMs[i] < -127) 
            motorPWMs[i] = -127;
    }
}

void motor_driver_node::reconfigureCallback(auv_motor_driver::auvConfig &config, uint32_t level)
{
    if (((std::string)config.port_name).compare(port) == 0) {
        if (state > auv_driver_node::CLOSED) {
       		driver->close();
       		state = auv_driver_node::CLOSED;
       	}
       	
        try {
            driver->open(((std::string)config.port_name).c_str(), 115200);
            state = auv_driver_node::OPENED;
            getDriverInfo();
        } catch (int e) {
            if (e == 19){
                ROS_ERROR("Open FAILED");
            }
            state = auv_driver_node::CLOSED;
        }
    }
}

void motor_driver_node::spin()
{
    ros::Rate loop_rate(50);
    while (!ros::isShuttingDown()) {
        if (!driver->isOpened()) {
            state = auv_driver_node::CLOSED;
        } else if (motorPWM_sub.getNumPublishers() == 1){
            uint8_t *currentSense;
            uint16_t errorCodes;
            if (((motor_driver *)driver)->setMotorSpeed(motorPWMs, &currentSense, &errorCodes) != 0) {
                ROS_ERROR("setMotorSpeed failed");
            } else {
                sendErrorCodesToDiagnostics(errorCodes);
                auv_msgs::MotorCurrent msg;
                for (int i = 0; i < 7; i++) {
    								msg.currents[i] = currentSense[i];
    						}
                motorCurrent_pub.publish(msg);
            }
        } else {
            uint8_t *currentSense;
            uint16_t errorCodes;
            if (((motor_driver *)driver)->setMotorSpeed(0,0,0,0,0,0, &currentSense, &errorCodes) != 0) {
                ROS_ERROR("setMotorSpeed failed");
            } else {
                sendErrorCodesToDiagnostics(errorCodes);
                auv_msgs::MotorCurrent msg;
                for (int i = 0; i < 7; i++) {
    								msg.currents[i] = currentSense[i];
    						}
                motorCurrent_pub.publish(msg);
            }
            ROS_WARN("Motor driver: Num publishers %d", motorPWM_sub.getNumPublishers());
        }
        spinOnce();
        loop_rate.sleep();
    }
    uint8_t *garbage;
    uint16_t trash;
    ((motor_driver *)driver)->setMotorSpeed(0,0,0,0,0,0, &garbage, &trash);
}

void motor_driver_node::update_motor_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
    if(errorOccured) {
        stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "AUV Motor Driver Diagnostics");
        stat.add("Drive H-Bridge", hbridgePresent[0]);
        stat.add("Verticle H-Bridge", hbridgePresent[1]);
        stat.add("Horizontal H-Bridge", hbridgePresent[2]);
        stat.add("Port Drive Motor", motorPresent[0]);
        stat.add("Starboard Drive Motor", motorPresent[1]);
        stat.add("Fore Verticle Motor", motorPresent[2]);
        stat.add("Aft Verticle Motor", motorPresent[3]);
        stat.add("Fore Horizontal Motor", motorPresent[4]);
        stat.add("Aft Horizontal Motor", motorPresent[5]);
        stat.add("Watchdog", watchdogReset);
        stat.add("Communication", communicationError);
    } else {
        stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "AUV Motor Driver Diagnostics");
        stat.add("Drive H-Bridge", hbridgePresent[0]);
        stat.add("Verticle H-Bridge", hbridgePresent[1]);
        stat.add("Horizontal H-Bridge", hbridgePresent[2]);
        stat.add("Port Drive Motor", motorPresent[0]);
        stat.add("Starboard Drive Motor", motorPresent[1]);
        stat.add("Fore Verticle Motor", motorPresent[2]);
        stat.add("Aft Verticle Motor", motorPresent[3]);
        stat.add("Fore Horizontal Motor", motorPresent[4]);
        stat.add("Aft Horizontal Motor", motorPresent[5]);
        stat.add("Watchdog", watchdogReset);
        stat.add("Communication", communicationError);
    }
}


void motor_driver_node::sendErrorCodesToDiagnostics(uint16_t errorCodes)
{
    uint8_t firstByte = (uint8_t)(errorCodes >> 8);
    uint8_t secondByte = (uint8_t)errorCodes;
    if (firstByte = 0x00){
        errorOccured = false;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "OK";
        }
        watchdogReset = "OK";
        communicationError = "OK";
    } else if ( firstByte = 0x01 ) {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            if (secondByte & 1 << i !=0)
                hbridgePresent[i] = "Disconnected";
            else
                hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "UNKNOWN";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "UNKNOWN";
        }
        watchdogReset = "UNKNOWN";
        communicationError = "UNKNOWN";
    } else if (firstByte = 0x02 ) {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            if (secondByte & 1 << i !=0)
                motorPresent[i] = "Disconnected";
            else
                motorPresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "UNKNOWN";
        }
        watchdogReset = "UNKNOWN";
        communicationError = "UNKNOWN";
    } else if (firstByte = 0x03 ) {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            if (secondByte & 1 << i !=0)
                motorOvercurrent[i] = "Overcurrent";
            else
                motorOvercurrent[i] = "OK";
        }
        watchdogReset = "UNKNOWN";
        communicationError = "UNKNOWN";
    } else if (firstByte = 0x04 ) {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "OK";
        }
        watchdogReset = "RESET";
        communicationError = "UNKNOWN";
    } else if (firstByte = 0x05 ) {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "OK";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "OK";
        }
        watchdogReset = "OK";
        communicationError = "ERROR";
    } else {
        errorOccured = true;
        for (int i = 0; i < 3; i++) {
            hbridgePresent[i] = "Communication ERROR";
        }
        for (int i = 0; i < 6; i++) {
            motorPresent[i] = "Communication ERROR";
        }
        for (int i = 0; i < 6; i++) {
            motorOvercurrent[i] = "Communication ERROR";
        }
        watchdogReset = "Communication ERROR";
        communicationError = "Communication ERROR";
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "auv_motor_driver");

    ros::NodeHandle n;
    
    motor_driver_node node(n);
    
    node.spin();
  
    return 0;
}
